# -*- coding: utf-8 -*-

import numpy as np
import weakref
import const

class Ellipse(object):

    def __init__(self, opt):
        self.var = ["a", "b", "yaw", "x0", "y0"]
        if opt is None:
            return
        for v in self.var:
            setattr(self, v, opt.variable())
        self.opt = weakref.ref(opt)
    
    def __call__(self, xy):
        x1, y1 = xy
        x, y = self.World2Local(x1, y1)
        return x**2/self.a**2 + y**2/self.b**2 - 1
    
    def BeforeSolve(self):
        opt = self.opt()
        opt.subject_to(self.a > 0.0)
        opt.subject_to(self.b > 0.0)
        opt.subject_to(self.a - self.b >= 0.0)
        opt.subject_to(self.yaw >= -np.pi / 2.0)
        opt.subject_to(self.yaw <= np.pi / 2.0)
        opt.set_initial(self.a, 1.0)
        opt.set_initial(self.b, 1.0)
        opt.set_initial(self.yaw, -np.pi / 2.0)
        opt.set_initial(self.x0, 0.0)
        opt.set_initial(self.y0, 0.0)
    
    def Solve(self):
        self.BeforeSolve()
        opt = self.opt()
        opt.solver('ipopt', const.OPTION)
        sol = opt.solve()
        self.AfterSolve(sol)

    def AfterSolve(self, sol):
        self.opt = None
        self.sol = sol
        for v in self.var:
            setattr(self, f"var_{v}", getattr(self, v))
            setattr(self, v, sol.value(getattr(self, f"var_{v}")))
    
    def Sample(self, t=None):
        if t is None:
            t = np.arange(0.0, np.pi * 2 + 0.1, 0.1)
        x = self.a * np.cos(t)
        y = self.b * np.sin(t)
        return self.Local2World(x, y)
    
    def Local2World(self, x, y):
        cos = np.cos(self.yaw)
        sin = np.sin(self.yaw)
        xt, yt = x, y
        x = cos * xt + sin * yt
        y = -sin * xt + cos * yt
        x += self.x0
        y += self.y0
        return x, y
    
    def World2Local(self, x1, y1):
        cos = np.cos(-self.yaw)
        sin = np.sin(-self.yaw)
        x = cos * (x1 - self.x0) + sin * (y1 - self.y0)
        y = -sin * (x1 - self.x0) + cos * (y1 - self.y0)
        return x, y
    
    def DistanceInUnitCircle(self, p, e):
        """
        求出把椭圆映射成单位圆后pe两点连接成直线与圆心的距离,
        用于判断椭圆与直线是否相切
        """
        xp, yp = self.World2Local(p[0], p[1])
        xe, ye = self.World2Local(e[0], e[1])
        xp /= self.a
        xe /= self.a
        yp /= self.b
        ye /= self.b
        A = yp - ye
        B = - (xp - xe)
        C = xp*ye - yp*xe
        d = np.sqrt(C**2/(A**2+B**2))
        return d
    
    def IsCross(self, p, e):
        """
        判断椭圆与pe两点连接成的线段是否与椭圆有交点
        """
        xp, yp = self.World2Local(p[0], p[1])
        xe, ye = self.World2Local(e[0], e[1])
        xp /= self.a
        xe /= self.a
        yp /= self.b
        ye /= self.b
        A = yp - ye
        B = - (xp - xe)
        C = xp*ye - yp*xe
        d = np.sqrt(C**2/(A**2+B**2))
        if d >= 1.0:
            return False
        cos = A / np.sqrt(A**2+B**2)
        sin = B / np.sqrt(A**2+B**2)
        y1 = -sin * xp + cos * yp
        y2 = -sin * xe + cos * ye
        y0 = np.sqrt(1 - d*d)
        if y1 <= -y0 and y2 <= -y0:
            return False
        if y1 >= y0 and y2 >= y0:
            return False
        return True